function [q_cal] = my_angle2quat(input)
% 采用远程火箭常用的坐标系转换方式 3-2-1 弧度
n = size(input,2);
q_cal = zeros(4,n);

for i=1:1:n
phi = input(1,i);psi = input(2,i);gamma = input(3,i);

C_BI = [cos(phi) * cos(psi), sin(phi)* cos(psi), -sin(psi);
cos(phi)* sin(psi)* sin(gamma) - sin(phi) * cos(gamma), sin(phi)* sin(psi)* sin(gamma) + cos(phi) * cos(gamma), cos(psi)* sin(gamma);
cos(phi)* sin(psi)* cos(gamma) + sin(phi) * sin(gamma), sin(phi)* sin(psi)* cos(gamma) - cos(phi) * sin(gamma), cos(psi)* cos(gamma)];

q_cal(1,i) = 0.5 * sqrt(1 + C_BI(1, 1) + C_BI(2, 2) + C_BI(3, 3));
q_cal(2,i) = (C_BI(2, 3) - C_BI(3, 2)) / (4 * q_cal(1,i));
q_cal(3,i) = (C_BI(3, 1) - C_BI(1, 3)) / (4 * q_cal(1,i));
q_cal(4,i) = (C_BI(1, 2) - C_BI(2, 1)) / (4 * q_cal(1,i));
end

end

